﻿//#include <unistd.h>
//#include <sys/types.h>
//#include <sys/wait.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>

#include "MediaPaser.h"
#include "host.h"

using namespace std;
using namespace host;

UInt16 CMediaParser::m_u16LastAudioSeq = 0;
UInt16 CMediaParser::m_u16FullAudioFrame = 0;
UInt32 CMediaParser::m_u32AudioRtpDataLen = 0;
char CMediaParser::m_acAudioRtpDataBuf[FULL_AUDIO_FRAME_MAX_SIZE] = { 0 };
UInt64 CMediaParser::m_u16RecvVideoCnt = 0;
UInt16 CMediaParser::m_u16RecvVideoSeq = 65535U;
UInt16 CMediaParser::m_u16LastVideoSeq = 65535U;
UInt16 CMediaParser::m_u16FullVideoFrame = 0;
UInt32 CMediaParser::m_u32VideoRtpDataLen = 0;
char CMediaParser::m_acVideoRtpDataBuf[FULL_VIDEO_FRAME_MAX_SIZE] = { 0 };
bool CMediaParser::m_bVideoFrameLost = false;
UInt16 CMediaParser::m_u16UacHidSeq = 0;
UInt16 CMediaParser::m_u16LastUacHidSeq = 0;
UInt16 CMediaParser::m_u16FullUacHidFrame = 0;
UInt32 CMediaParser::m_u32UacHidDataLen = 0;
char CMediaParser::m_acUacHidDataBuf[FULL_AUDIO_FRAME_MAX_SIZE] = { 0 };
list<TRtpDataInfo*> CMediaParser::m_lVideoPacketFree;
list<TRtpDataInfo*> CMediaParser::m_lVideoPacketBusy;



unsigned int GetUInt32(unsigned char*& i_pBufPos)
{
    unsigned int* pValue = reinterpret_cast<unsigned int*>(i_pBufPos);
    i_pBufPos += sizeof(unsigned int);
    return OS::Ntohl(*pValue);
}

unsigned int GetUInt16(unsigned char*& i_pBufPos)
{
    unsigned short* pValue = reinterpret_cast<unsigned short*>(i_pBufPos);
    i_pBufPos += 2;
    return OS::Ntohs (*pValue);
}

unsigned int GetUInt8(unsigned char*& i_pBufPos)
{
    unsigned char* pValue = reinterpret_cast<unsigned char*>(i_pBufPos);
    i_pBufPos += 1;
    return *pValue;
}


CMediaParser::CMediaParser()
{

}

CMediaParser::~CMediaParser()
{

}


int CMediaParser::Init()
{
    for (size_t i = 0; i < MAX_RTP_PACK_BUF_NUM; i++)
    {
        TRtpDataInfo *pRtpData = new TRtpDataInfo;
        m_lVideoPacketFree.push_back(pRtpData);
    }

    return 0;
}


int CMediaParser::Reset()
{
    while (m_lVideoPacketBusy.size())
    {
        TRtpDataInfo *pRtpData = m_lVideoPacketBusy.front();
        m_lVideoPacketFree.push_back(pRtpData);
        m_lVideoPacketBusy.pop_front();
    }

    return 0;
}


int CMediaParser::Destroy()
{
    while (m_lVideoPacketBusy.size())
    {
        TRtpDataInfo *pRtpData = m_lVideoPacketBusy.front();
        delete pRtpData;
        m_lVideoPacketBusy.pop_front();
    }
    while (m_lVideoPacketFree.size())
    {
        TRtpDataInfo *pRtpData = m_lVideoPacketFree.front();
        delete pRtpData;
        m_lVideoPacketFree.pop_front();
    }

    return 0;
}


TRtpDataInfo * CMediaParser::GetBuffer()
{
    TRtpDataInfo *pRtpData = NULL;
    if (m_lVideoPacketFree.size())
    {
        pRtpData = m_lVideoPacketFree.front();
        return pRtpData;
    }

    return NULL;
}


bool CMediaParser::AudioRtpPacketParse(unsigned char* i_pBuf, UInt32 i_u32BufLen, char** o_pDataPtr, UInt32* o_pDataLen, UInt32* o_u32Timestamp)
{
    if (NULL == i_pBuf || i_u32BufLen < PAYLOAD_AUDIO_RTPHEAD_SIZE || i_pBuf[0] != 0x90 || i_pBuf[1] != E_PT_MIC_PCM)
    {
        BGLog(LL_ERROR, "invalid usb mic rtp packet");
        return false;
    }

    unsigned char* pBuf = i_pBuf + 2;
    UInt16 u16Seq = GetUInt16(pBuf);
    if (u16Seq != static_cast<UInt16>(1 + m_u16LastAudioSeq))
    {
        BGLog(LL_WARN, "recv usb mic rtp packet lost from <%u> to <%u>.", m_u16LastAudioSeq, u16Seq);
    }
    m_u16LastAudioSeq = u16Seq;

    UInt32 u32Timestamp = GetUInt32(pBuf); // timestamp
    *o_u32Timestamp = u32Timestamp;

    GetUInt32(pBuf);   // ssrc

    GetUInt16(pBuf);   // rtp extend_profile
    GetUInt16(pBuf);   // rtp extend_len
    GetUInt8(pBuf);    // rtp extend_version

    UInt8 u8PType = GetUInt8(pBuf);

    GetUInt16(pBuf);   // reserved

    UInt32 u32RtpDataLen = i_u32BufLen - PAYLOAD_AUDIO_RTPHEAD_SIZE;

    switch (u8PType)
    {
        case MEDIA_PK_FULL:
            memcpy(m_acAudioRtpDataBuf, i_pBuf + PAYLOAD_AUDIO_RTPHEAD_SIZE, u32RtpDataLen);
            m_u32AudioRtpDataLen = u32RtpDataLen;
            m_u16FullAudioFrame = 1;
            break;
        case MEDIA_PK_START:
            memcpy(m_acAudioRtpDataBuf, i_pBuf + PAYLOAD_AUDIO_RTPHEAD_SIZE, u32RtpDataLen);
            m_u32AudioRtpDataLen = u32RtpDataLen;
            m_u16FullAudioFrame = 0;
            return false;
        case MEDIA_PK_MIDDLE:
        case MEDIA_PK_END:
            if (m_u16FullAudioFrame != 0)
            {
                return false;
            }
            memcpy(m_acAudioRtpDataBuf + m_u32AudioRtpDataLen, i_pBuf + PAYLOAD_AUDIO_RTPHEAD_SIZE, u32RtpDataLen);
            m_u32AudioRtpDataLen += u32RtpDataLen;
            if (u8PType == MEDIA_PK_END)
            {
                m_u16FullAudioFrame = 1;
                break;
            }
            return false;
        default:
            BGLog(LL_WARN, "unknown package type: %d, discard.", u8PType);
            return false;
    }

    BGLog(LL_DEBUG, "AudioRtpPacketParse type: %d, RtpDataLen: %d, TotalRtpDataLen: %d", u8PType, u32RtpDataLen, m_u32AudioRtpDataLen);

    *o_pDataPtr = m_acAudioRtpDataBuf;
    *o_pDataLen = m_u32AudioRtpDataLen;
    m_u32AudioRtpDataLen = 0;

    return true;
}

bool CMediaParser::AudioG722PacketParse(unsigned char* i_pBuf, UInt32 i_u32BufLen, char** o_pDataPtr, UInt32* o_pDataLen, UInt32* o_u32Timestamp)
{
    if (NULL == i_pBuf || i_u32BufLen < PAYLOAD_AUDIO_RTPHEAD_SIZE || i_pBuf[0] != 0x90 || i_pBuf[1] != E_PT_MIC_PCM)
    {
        BGLog(LL_ERROR, "invalid usb mic rtp packet");
        return false;
    }

    unsigned char* pBuf = i_pBuf + 2;
    UInt16 u16Seq = GetUInt16(pBuf);
    if (u16Seq != static_cast<UInt16>(1 + m_u16LastAudioSeq))
    {
        BGLog(LL_WARN, "recv usb mic rtp packet lost from <%u> to <%u> i_u32BufLen:%d.", m_u16LastAudioSeq, u16Seq, i_u32BufLen);
    }

    UInt32 u32Timestamp = GetUInt32(pBuf); // timestamp
    GetUInt32(pBuf);   // ssrc
    GetUInt16(pBuf);   // rtp extend_profile
    GetUInt16(pBuf);   // rtp extend_len
    GetUInt8(pBuf);    // rtp extend_version
    UInt8 u8PType = GetUInt8(pBuf);
    GetUInt16(pBuf);   // reserved

    if (o_u32Timestamp)
    {
        *o_u32Timestamp = u32Timestamp;
    }

    UInt32 u32RtpDataLen = i_u32BufLen - PAYLOAD_AUDIO_RTPHEAD_SIZE;
    UInt32 u32Frames = u32RtpDataLen / 80;
    UInt16 u16LossCnt = (u16Seq - m_u16LastAudioSeq) & 0xffff;
    if (u16LossCnt > 80U)
        u16LossCnt = 1;
    else if (u16LossCnt >= u32Frames)
        u16LossCnt = u32Frames;

    m_u16LastAudioSeq = u16Seq;

    if (u16LossCnt == 0) //repeat
    {
        return false;
    }
    if (u16LossCnt == 1)
    {
        memcpy(m_acAudioRtpDataBuf, i_pBuf + PAYLOAD_AUDIO_RTPHEAD_SIZE, 80);
        m_u32AudioRtpDataLen = 80;
        m_u16FullAudioFrame = 1;
    }
    else
    {
        m_u32AudioRtpDataLen = 0;
        for (decltype(u32Frames) i = 0; i < u16LossCnt && i < u32Frames; i++)
        {
            memcpy(m_acAudioRtpDataBuf + i * 80, i_pBuf + PAYLOAD_AUDIO_RTPHEAD_SIZE + 80 * (u16LossCnt - i - 1), 80);
            m_u32AudioRtpDataLen += 80;
        }
        m_u16FullAudioFrame = 1;
    }

    if (u16LossCnt > 1)
    {
        BGLog(LL_DEBUG, "AudioRtpPacketParse type: %d, RtpDataLen: %d, TotalRtpDataLen: %d", u8PType, u32RtpDataLen, m_u32AudioRtpDataLen);
    }

    *o_pDataPtr = m_acAudioRtpDataBuf;
    *o_pDataLen = m_u32AudioRtpDataLen;
    m_u32AudioRtpDataLen = 0;

    return true;
}

bool CMediaParser::RtpSeqCompare(UInt16 i_u16Seq1, UInt16 i_u16Seq2)
{
    int Seq1 = i_u16Seq1;
    int Seq2 = i_u16Seq2;
    if (Seq1 < 64 && Seq2 >(65536 - 64))
        return false;
    if (Seq1 > (65536 - 64) && Seq2 < 64)
        return true;
    return Seq1 < Seq2;
}

bool CMediaParser::RtpDataCompare(TRtpDataInfo *i_tFrontRtpData, TRtpDataInfo *i_tBackRtpData)
{
    return RtpSeqCompare(i_tFrontRtpData->u32PackSeq, i_tBackRtpData->u32PackSeq);
}

bool CMediaParser::HandleDisorderRtpData(TRtpDataInfo *i_ptRtpData, TRtpDataInfo* &o_ptRtpData)
{
    if (NULL == i_ptRtpData)
        return false;

    m_lVideoPacketFree.pop_front();
    m_lVideoPacketBusy.push_back(i_ptRtpData);
    m_lVideoPacketBusy.sort(RtpDataCompare); // reorder, fix network disorder

    if (m_bVideoFrameLost) // 解析START分片后会清除
    {
        // 重新搜索START分片
        UInt16 u16LastSeq = m_u16LastVideoSeq;
        while (m_lVideoPacketBusy.size())
        {
            TRtpDataInfo *tCurFrontData = m_lVideoPacketBusy.front();
            if (tCurFrontData->u8PackType == MEDIA_PK_START || tCurFrontData->u8PackType == MEDIA_PK_FULL)
            {
                m_lVideoPacketBusy.pop_front();
                m_lVideoPacketFree.push_back(tCurFrontData);
                o_ptRtpData = tCurFrontData;
                return true;
            }
            if (tCurFrontData->u32PackSeq == ((1 + u16LastSeq) & 0xffff))//check continuous
            {
                m_lVideoPacketBusy.pop_front();
                m_lVideoPacketFree.push_back(tCurFrontData);
                u16LastSeq = tCurFrontData->u32PackSeq;
            }
            else
            {
                //不连续时，可能还有数据未到达，等待下一次处理
                break;
            }
        }
    }

    if (m_lVideoPacketBusy.size() == 0)
    {
        return false;
    }

    TRtpDataInfo *tCurFrontData = m_lVideoPacketBusy.front();
    if (tCurFrontData->u32PackSeq == ((1 + m_u16LastVideoSeq) & 0xffff))//check continuous
    {
        m_lVideoPacketBusy.pop_front();
        m_lVideoPacketFree.push_back(tCurFrontData);
        o_ptRtpData = tCurFrontData;
        return true;
    }

    if (m_lVideoPacketBusy.size() >= MAX_RTP_PACK_BUF_NUM) //list full
    {
        if (!m_bVideoFrameLost)
        {
            BGLog(LL_DEBUG, "usb camera rtp packet lost <%u> to <%u>.", m_u16LastVideoSeq, tCurFrontData->u32PackSeq);
        }
        m_bVideoFrameLost = true; //丢包了

        m_lVideoPacketBusy.pop_front();
        m_lVideoPacketFree.push_back(tCurFrontData);
        if (RtpSeqCompare(m_u16LastVideoSeq, tCurFrontData->u32PackSeq)
            || (UInt16)(tCurFrontData->u32PackSeq - m_u16LastVideoSeq) > MAX_RTP_PACK_BUF_NUM) //lost
        {
            BGLog(LL_INFO, "usb camera rtp packet seq change <%u> to <%u>.", m_u16LastVideoSeq, tCurFrontData->u32PackSeq);

            //如果是START分片，直接返回
            if (tCurFrontData->u8PackType == MEDIA_PK_START || tCurFrontData->u8PackType == MEDIA_PK_FULL)
            {
                o_ptRtpData = tCurFrontData;
                return true;
            }

            // 重新搜索START分片，START分片前的都是不完整的数据
            UInt16 u16LastSeq = tCurFrontData->u32PackSeq;
            while (m_lVideoPacketBusy.size())
            {
                TRtpDataInfo *tCurFrontData = m_lVideoPacketBusy.front();
                if (tCurFrontData->u8PackType == MEDIA_PK_START || tCurFrontData->u8PackType == MEDIA_PK_FULL)
                {
                    m_lVideoPacketBusy.pop_front();
                    m_lVideoPacketFree.push_back(tCurFrontData);
                    o_ptRtpData = tCurFrontData;
                    return true;
                }
                if (tCurFrontData->u32PackSeq == ((1 + u16LastSeq) & 0xffff))//check continuous
                {
                    m_lVideoPacketBusy.pop_front();
                    m_lVideoPacketFree.push_back(tCurFrontData);
                    u16LastSeq = tCurFrontData->u32PackSeq;
                }
                else
                {
                    //不连续时，可能还有数据未到达，等待下一次处理
                    m_u16LastVideoSeq = tCurFrontData->u32PackSeq;
                    return false;
                }
            }
            return false;
        }
        else //Rollback , drop it
        {
            return false;
        }
    }

    return false;
}

bool CMediaParser::HandleRtpPackType(TRtpDataInfo *i_ptRtpData)
{
    if (i_ptRtpData == NULL)
    {
        return false;
    }

    switch (i_ptRtpData->u8PackType)
    {
        case MEDIA_PK_FULL:
        {
            if (m_bVideoFrameLost)
            {
                m_bVideoFrameLost = false;
            }
            memcpy(m_acVideoRtpDataBuf, i_ptRtpData->pRtpPayload, i_ptRtpData->u32PayloadLen);
            m_u32VideoRtpDataLen = i_ptRtpData->u32PayloadLen;
            m_u16FullVideoFrame = 1;
            return true;
        }
        case MEDIA_PK_START:
        {
            if (m_bVideoFrameLost)
            {
                m_bVideoFrameLost = false;
            }
            memcpy(m_acVideoRtpDataBuf, i_ptRtpData->pRtpPayload, i_ptRtpData->u32PayloadLen);
            m_u32VideoRtpDataLen = i_ptRtpData->u32PayloadLen;
            m_u16FullVideoFrame = 0;

            return false;
        }
        case MEDIA_PK_MIDDLE:
        case MEDIA_PK_END:
        {
            if (m_bVideoFrameLost)
            {
                return false;
            }
            if (m_u16FullVideoFrame != 0)
            {
                BGLog(LL_ERROR, "MEDIA_PK_MIDDLE or MEDIA_PK_END start, not a full frame, drop it");
                return false;
            }
            memcpy(m_acVideoRtpDataBuf + m_u32VideoRtpDataLen, i_ptRtpData->pRtpPayload, i_ptRtpData->u32PayloadLen);
            m_u32VideoRtpDataLen += i_ptRtpData->u32PayloadLen;
            if (i_ptRtpData->u8PackType == MEDIA_PK_END)
            {
                m_u16FullVideoFrame = 1;
                return true;
            }

            return false;
        }
        default:
            BGLog(LL_WARN, "unknown package type: %d, discard.", i_ptRtpData->u8PackType);
            return false;
    }

    return false;
}

bool CMediaParser::VideoRtpPacketParse(TRtpDataInfo *pRtpData, char** o_pDataPtr, UInt32 &o_u32FrameLen)
{
    if (NULL == pRtpData || pRtpData->u32RtpDataLen < PAYLOAD_AUDIO_RTPHEAD_SIZE)
    {
        BGLog(LL_ERROR, "invalid usb camera rtp packet");
        return false;
    }
    if (pRtpData->acRtpData[0] != 0x90 || pRtpData->acRtpData[1] != E_PT_CAM_MJPEG)
    {
        BGLog(LL_ERROR, "invalid usb camera rtp packet %02 %02x", pRtpData->acRtpData[0], pRtpData->acRtpData[1]);
        return false;
    }

    bool bFrameFull = false;
    bool bPopRtpData = false;
    TRtpDataInfo *tCurRtpData = NULL;

    unsigned char* pBuf = pRtpData->acRtpData + 2;
    UInt16 u16Seq = GetUInt16(pBuf);
    pRtpData->u32Timestamp = GetUInt32(pBuf); // timestamp

    GetUInt32(pBuf);   // ssrc

    uint16_t u16ExtPrf = GetUInt16(pBuf);   // rtp extend_profile
    uint16_t u16ExtLen = GetUInt16(pBuf);   // rtp extend_len
    uint8_t  u8ExtVer  = GetUInt8(pBuf);    // rtp extend_version
    pRtpData->u8PackType = GetUInt8(pBuf);
    GetUInt16(pBuf);   // reserved

    pRtpData->u32PayloadLen = pRtpData->u32RtpDataLen - (RTP_COM_HEARD_SIZE + 4 + u16ExtLen * 4);
    pRtpData->pRtpPayload = pRtpData->acRtpData + (RTP_COM_HEARD_SIZE + 4 + u16ExtLen * 4);

    if (m_u16RecvVideoCnt == 0)
    {
        BGLog(LL_WARN, "recv usb camera first rtp packet <%u>.", u16Seq);
        m_u16RecvVideoSeq = u16Seq - 1;
        m_u16LastVideoSeq = u16Seq - 1;
    }
    m_u16RecvVideoCnt++;

    if (u16Seq != ((1U + m_u16RecvVideoSeq) & 0xffff))
    {
        BGLog(LL_WARN, "recv usb camera rtp packet lost from <%u> to <%u>.", m_u16RecvVideoSeq, u16Seq);
    }
    m_u16RecvVideoSeq = u16Seq;

    pRtpData->u32PackSeq = u16Seq;
    if (!HandleDisorderRtpData(pRtpData, tCurRtpData))
    {
        return false;
    }

    //BGLog(LL_DEBUG, "disorder usb camera rtp packet from <%u> to <%u>.", m_u16LastVideoSeq, tCurRtpData->u32PackSeq);
    m_u16LastVideoSeq = tCurRtpData->u32PackSeq;

    bFrameFull = HandleRtpPackType(tCurRtpData);
    if (!bFrameFull)
    {
        if (m_lVideoPacketBusy.size() == 0)
        {
            return false;
        }

        while (m_lVideoPacketBusy.size())
        {
            tCurRtpData = m_lVideoPacketBusy.front();
            if (tCurRtpData->u32PackSeq != ((m_u16LastVideoSeq + 1) & 0xffff))
            {
                bFrameFull = false;
                break;
            }
            BGLog(LL_DEBUG, "LastVideoSeq%d, RtpData%d type: %d.", m_u16LastVideoSeq, tCurRtpData->u32PackSeq, tCurRtpData->u8PackType);
            m_u16LastVideoSeq = tCurRtpData->u32PackSeq;

            bFrameFull = HandleRtpPackType(tCurRtpData);
            m_lVideoPacketBusy.pop_front();
            m_lVideoPacketFree.push_back(tCurRtpData);
            if (bFrameFull)
            {
                break;
            }
        }
        if (!bFrameFull)
        {
            return false;
        }
    }

    BGLog(LL_DEBUG, "VideoRtpPacketParse type: %d, RtpDataLen: %d, TotalRtpDataLen: %d.",
        tCurRtpData->u8PackType, tCurRtpData->u32RtpDataLen, m_u32VideoRtpDataLen);

    if (o_pDataPtr)
        *o_pDataPtr = m_acVideoRtpDataBuf;
    o_u32FrameLen = m_u32VideoRtpDataLen;
    pRtpData->u32Timestamp = tCurRtpData->u32Timestamp;
    m_u32VideoRtpDataLen = 0;

    return true;
}

bool CMediaParser::VideoRtpPacketReset()
{
    CMediaParser::m_u16RecvVideoCnt = 0;
    CMediaParser::m_u16RecvVideoSeq = 65535U;
    CMediaParser::m_u16LastVideoSeq = 65535U;
    CMediaParser::m_u16FullVideoFrame = 0;

    Reset();

    return true;
}

void CMediaParser::GetState(UInt32 &nFreeNr, UInt32 &nBusyNr)
{
    nFreeNr = (UInt32)m_lVideoPacketFree.size();
    nBusyNr = (UInt32)m_lVideoPacketBusy.size();
}
